﻿//#include"winsock2.h"
#include <vector>
#include <list>
#include <cmath>
#include <iostream>
#include <time.h>
#include <cstdlib>
#include <thread>
#include <math.h>
#include <algorithm>
#include <string>
#include <stdlib.h>
#include <mutex>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/multi_point.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>
//这是linux下的 暂时先这样写
//#include <unistd.h>
namespace bg = boost::geometry;
namespace bgm = bg::model;
BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian)

typedef boost::tuple<double, double> point;
typedef bgm::d2::point_xy<double> point_xy;

typedef bgm::multi_point<point_xy> points;

typedef bgm::polygon<point_xy> hull_point;
typedef bgm::polygon<point_xy> polygon;

typedef unsigned char uint8_t;

void usleep(unsigned long usec); //这是liux中的函数

using namespace std;

#define PI 3.1415926                  //圆周率
const double EARTH_RADIUS = 6378.137; //地球半径
//先定义一个起始坐标点
#define START_LON 125.1631713
#define START_LAT 43.8324610
//车辆和路侧检测到的障碍物数据
//进行冗余检测时需要用到的数据1
struct map_world
{
    float lon;
    float lat;
};
struct coordinate
{
    float x;
    float y;
};
struct bbox
{
    //时间戳
    double counter_time;
    //类别
    long classification;
    //置信度
    float score;
    //将四个顶点坐标存到这个数组中
    coordinate apex_coordinate[4];
    //存中心点坐标
    coordinate center_coordinate;
    //长宽高
    float height;
    float length;
    float width;
    //障碍物航向角
    float heading;
    //障碍物速度
    float speed_object;
    //障碍物横纵向速度
    float x_speed;
    float y_speed;
    //障碍物经纬度
    float lon;
    float lat;
};
static double rad(double d)
{
    return d * PI / 180;
}
//根据经纬度求两点间距离
float cacu_gps_dis(map_world H_map, map_world R_map)
{
    double radLat1 = rad(H_map.lat);
    double radLat2 = rad(R_map.lat);
    double a = radLat1 - radLat2;
    double b = rad(H_map.lon) - rad(R_map.lon);
    double s = 2 * asin(sqrt(pow(sin(a / 2), 2) + cos(radLat1) * cos(radLat2) * pow(sin(b / 2), 2)));
    s = s * EARTH_RADIUS;
    s = s * 1000;
    return s;
}
//根据经纬度求两点之间夹角
float get_angle(map_world map_a, map_world map_b)
{
    double x = map_a.lat - map_b.lat;
    double y = map_a.lon - map_b.lon;
    int angle = -1;
    if (y == 0 && x > 0)
        angle = 0;
    if (y == 0 && x < 0)
        angle = 180;
    if (x == 0 && y > 0)
        angle = 90;
    if (x == 0 && y < 0)
        angle = 270;
    if (angle == -1)
    {
        map_world map_c, map_d;
        map_c.lon = map_b.lon;
        map_c.lat = map_a.lat;
        map_d.lon = map_a.lon;
        map_d.lat = map_b.lat;
        double dislat = cacu_gps_dis(map_c, map_b);
        double dislng = cacu_gps_dis(map_d, map_b);
        if (x > 0 && y > 0)
            angle = atan2(dislng, dislat) / PI * 180;
        if (x < 0 && y > 0)
            angle = atan2(dislat, dislng) / PI * 180 + 90;
        if (x < 0 && y < 0)
            angle = atan2(dislng, dislat) / PI * 180 + 180;
        if (x > 0 && y < 0)
            angle = atan2(dislat, dislng) / PI * 180 + 270;
    }
    return angle;
}
/**
 * @brief 将point数组转为内部使用的格式
 *
 * @param p point array
 * @return points
 */
points convert(vector<point> p)
{
    points res;
    for (auto i = p.begin(); i != p.end(); i++)
    {
        res.push_back(point_xy(i->head, i->tail.head));
    }
    return res;
}

/**
 * @brief Get the Hull Points object, 求出顺时针的凸包
 *
 * @param p 任意顺序的四个点
 * @return bgm::polygon<point>
 */
hull_point getHullPoints(points &p)
{
    bgm::polygon<point_xy> hull;
    bg::convex_hull(p, hull);
    return hull;
}

/**
 * @brief Get the Intersection Area object
 *
 * @param rect1 第一个长方形
 * @param rect2 第二个长方形
 * @return double
 */
double getIntersectionArea(polygon rect1, polygon rect2)
{
    std::deque<polygon> output;
    bg::intersection(rect1, rect2, output);
    auto polygon_output = output.begin();
    if (output.size()==0)
	{
		return 0;
	}
	else
	{
		return bg::area(*polygon_output);
	}
}

/**
 * @brief Get the Union object
 *
 * @param rect1
 * @param rect2
 * @return double
 */
double getUnion(polygon rect1, polygon rect2)
{
    return bg::area(rect1) + bg::area(rect2);
}

/**
 * @brief
 *
 * @param rect1 {{-1, 0}, {1, 1}, {1, 0}, {-1, 1}}
 * @param rect2 {{0, 0}, {1, 1}, {0, 2}, {-1, 1}}
 * @return double
 */
double get_IOU(vector<point> rect1, vector<point> rect2)
{
    //转换格式
    points rect1_point = convert(rect1);
    points rect2_point = convert(rect2);
    //将两个矩形变成凸包
    auto hull1 = getHullPoints(rect1_point);
    auto hull2 = getHullPoints(rect2_point);
    //求交集
    auto area_intersection = getIntersectionArea(hull1, hull2);
    //求并集
    auto area_union = getUnion(hull1, hull2) - area_intersection;

    return area_union / area_intersection;
}
//计算自车、路侧检测到的障碍物中心点坐标
void cacu_center_xy(vector<bbox> &b)
{
    float distance,azimuth;
    map_world start_lon_lat, end_lon_lat;
    start_lon_lat.lon = START_LON;
    start_lon_lat.lat = START_LAT;
    
    for (int i = 0; i < b.size(); i++)
    {
        end_lon_lat.lon = b[i].lon;
        end_lon_lat.lat = b[i].lat;
        distance = cacu_gps_dis(start_lon_lat, end_lon_lat);
        azimuth = rad(get_angle(start_lon_lat, end_lon_lat));
        b[i].center_coordinate.x = cos(azimuth) *distance;
        b[i].center_coordinate.y = sin(azimuth) *distance;
    }
}
//计算障碍物信息计算坐标
void cacu_pos_apex(bbox &box1)
{
    //路端和车端的heading指的是路侧检测到的障碍物的heading
    //先将障碍物和世界坐标系方向一致
    float theta = box1.heading;
    float rotation_matrix[2][2];
    rotation_matrix[0][0] = cos(theta);
    rotation_matrix[0][1] = -sin(theta);
    rotation_matrix[1][0] = sin(theta);
    rotation_matrix[1][1] = cos(theta);
    box1.apex_coordinate[0].x = -rotation_matrix[0][0] * box1.width / 2 + rotation_matrix[0][1] * box1.length / 2;
    box1.apex_coordinate[0].y = -rotation_matrix[1][0] * box1.width / 2 + rotation_matrix[1][1] * box1.length / 2;
    box1.apex_coordinate[1].x = rotation_matrix[0][0] * box1.width / 2 + rotation_matrix[0][1] * box1.length / 2;
    box1.apex_coordinate[1].y = rotation_matrix[1][0] * box1.width / 2 + rotation_matrix[1][1] * box1.length / 2;
    box1.apex_coordinate[3].x = -rotation_matrix[0][0] * box1.width / 2 - rotation_matrix[0][1] * box1.length / 2;
    box1.apex_coordinate[3].y = -rotation_matrix[1][0] * box1.width / 2 - rotation_matrix[1][1] * box1.length / 2;
    box1.apex_coordinate[2].x = rotation_matrix[0][0] * box1.width / 2 - rotation_matrix[0][1] * box1.length / 2;
    box1.apex_coordinate[2].y = rotation_matrix[1][0] * box1.width / 2 - rotation_matrix[1][1] * box1.length / 2;
    //进行平移
    //这里的经纬度也是指的路侧检测到的障碍物的经纬度
    //以世界坐标系为起点进行平移
    //平移变换
    for (int i = 0; i < 4; i++)
    {
        box1.apex_coordinate[i].x = box1.apex_coordinate[i].x + box1.center_coordinate.x;
        box1.apex_coordinate[i].y = box1.apex_coordinate[i].y + box1.center_coordinate.y;
    }
}
bool comp(const bbox &box1, const bbox &box2)
{
    return box1.score > box2.score;
}
//根据置信度进行排序,决定比较顺序
void NMS(vector<bbox> &bb)
{
    //根据置信度进行排序
    sort(bb.begin(), bb.end(), comp);
    vector<bbox>::iterator ib = bb.begin();
    while (ib != bb.end())
    {
        vector<bbox>::iterator it = ib;
        it++;
        while (it != bb.end())
        {
            //对每一个需要进行求iou的矩形框求出坐标
            cacu_pos_apex(*it);
            cacu_pos_apex(*ib);
            //存入point中
            vector<point> comp_rect1, comp_rect2;
            for (size_t i = 0; i < 4; i++)
            {
                comp_rect1.push_back({(*ib).apex_coordinate[i].x, (*ib).apex_coordinate[i].y});
                comp_rect2.push_back({(*it).apex_coordinate[i].x, (*it).apex_coordinate[i].y});
            }
            comp_rect1.push_back({(*ib).apex_coordinate[0].x, (*ib).apex_coordinate[0].y});
            comp_rect2.push_back({(*it).apex_coordinate[0].x, (*it).apex_coordinate[0].y});
            //当iou大于阈值时，将删除此box，进行下一个比较
            if (get_IOU(comp_rect1, comp_rect2) > 0.6)
            {
                it = bb.erase(it);
            }
            else
            {
                it++;
            }
        }
        ib++;
    }
}
//信息要存在vector中，因为不知道是多少
//根据坐标信息来改id即，算属于第几个方块
//b1是车端收到的障碍物列表，b2是路侧的障碍物列表
//同时对是来自车端还是路端的消息进行标识
void con_square(vector<bbox> b1, vector<bbox> b2, vector<vector<bbox>> &divide_obstacle_list)
{
    int row_b, line_b; //方块的行列数目
    //k表示该中心点位置在第行，b表示在第几行的第几列,id表示最终属于哪个方块中
    int k, b, id;
    //先计算世界坐标系下出车端和路端的中心点坐标
    cacu_center_xy(b1);
    cacu_center_xy(b2);
    float min_xx = b1[0].center_coordinate.x, max_xx = b1[0].center_coordinate.x, min_yy = b1[0].center_coordinate.y, max_yy = b1[0].center_coordinate.y;
    //先求b1里面的最小和最大
    for (int i = 0; i < b1.size(); i++)
    {
        if (b1[i].center_coordinate.x <= min_xx)
        {
            min_xx = b1[i].center_coordinate.x;
        }
        else if (b1[i].center_coordinate.x >= max_xx)
        {
            max_xx = b1[i].center_coordinate.x;
        }
        if (b1[i].center_coordinate.y <= min_yy)
        {
            min_yy = b1[i].center_coordinate.y;
        }
        else if (b1[i].center_coordinate.y >= max_yy)
        {
            max_yy = b1[i].center_coordinate.y;
        }
    }
    //再把最小的和b2去比较 因为要找总体的最小最大值来确定分为几个方格
    for (int i = 0; i < b2.size(); i++)
    {
        if (b2[i].center_coordinate.x <= min_xx)
        {
            min_xx = b2[i].center_coordinate.x;
        }
        else if (b2[i].center_coordinate.x >= max_xx)
        {
            max_xx = b2[i].center_coordinate.x;
        }
        if (b2[i].center_coordinate.y <= min_yy)
        {
            min_yy = b2[i].center_coordinate.y;
        }
        else if (b2[i].center_coordinate.y >= max_yy)
        {
            max_yy = b2[i].center_coordinate.y;
        }
    }
    //求出最小最大值，就可以看分为几乘几的方块了,取更大值
    //车长是443cm，车宽是159cm
    //求一行几个方块
    row_b = int(ceil((max_xx - min_xx) / 1.59f));
    //一列几个方块
    line_b = int(ceil((max_yy - min_yy) / 4.43f));
    divide_obstacle_list.resize(row_b * line_b);
    //算出车端障碍物在哪个方块中，存到相应容器中，同时对车端的消息进行标识来源
    for (int i = 0; i < b1.size(); i++)
    {
        k = (max_yy - b1[i].center_coordinate.y) / 4.43f;
        b = (b1[i].center_coordinate.x - min_xx) / 1.59f + 1;
        if (k == 0)
        {
            id = b;
        }
        else
        {
            if ((max_yy - b1[i].center_coordinate.y) - 4.43f * k <= 1e-6) //表示刚好整除
            {
                id = (k - 1) * row_b + b;
            }
            else
            {
                id = k * row_b + b;
            }
        }
        divide_obstacle_list[id - 1].push_back(b1[i]);
    }
    //对其他车的障碍物信息列表标识id
    for (int i = 0; i < b2.size(); i++)
    {
        k = (max_yy - b2[i].center_coordinate.y) / 4.43f;
        b = (b2[i].center_coordinate.x - min_xx) / 1.59f + 1;
        if (k == 0)
        {
            id = b;
        }
        else
        {
            if ((max_yy - b2[i].center_coordinate.y) - 4.43f * k <= 1e-6) //表示正好在上一行的下边
            {
                id = (k - 1) * row_b + b;
            }
            else
            {
                id = k * row_b + b;
            }
        }
        divide_obstacle_list[id - 1].push_back(b2[i]);
    }
}
//这里假设car_obtacle_list是车端障碍物列表
//road_obtacle_list是路侧障碍物列表
//函数类型和参数类型都改成rsm格式
//此时是一个进程，用void类型
void Merge(vector<bbox> car, vector<bbox> road)
{
    //假如传的是vector类型的，先这样写一下，主要是为了写上计算distance和azmuith和x_speed和y_speed的步骤
    vector<bbox> car_obtacle_list, road_obtacle_list;
    bbox temporary_box;
    // map_world start_lon_lat, end_lon_lat;
    // start_lon_lat.lon = START_LON;
    // start_lon_lat.lat = START_LAT;
    //将主车和路侧的车端信息赋值给定义的结构体
    for (int i = 0; i < car.size(); i++)
    {
        //end_lon_lat.lon = car[i].lon;
        //end_lon_lat.lat = car[i].lat;
        //temporary_box.distance = cacu_gps_dis(start_lon_lat, end_lon_lat);
       // temporary_box.azimuth = rad(get_angle(start_lon_lat, end_lon_lat));
        temporary_box.heading = rad(car[i].heading);
        temporary_box.speed_object = car[i].speed_object;
        temporary_box.x_speed = cos(temporary_box.heading) * temporary_box.speed_object;
        temporary_box.y_speed = sin(temporary_box.heading) * temporary_box.speed_object;
    }
    //确定在哪个方块内
    vector<vector<bbox>> merge_obstacle_list; //外层vector表示划分为一维数组的几个方块，内层的vector是保存每个方块内的bbox
    con_square(car_obtacle_list, road_obtacle_list, merge_obstacle_list);
    vector<bbox> cls;
    //将标识一样的暂时保存到cls这个vector里面去做nms操作
    //最终输出的融合后的障碍物列表
    vector<bbox> final_list;
    for (int i = 0; i < merge_obstacle_list.size(); i++)
    {
        //将此方块中的数据放到cls中
        cls.insert(cls.end(), merge_obstacle_list[i].begin(), merge_obstacle_list[i].end());
        //存完之后，对这个方块内的数据进行nms操作
        NMS(cls);
        //nms操作后的cls保存到一个新的vector中，就是最终融合后的障碍物列表
        final_list.insert(final_list.end(), cls.begin(), cls.end());
        vector<bbox>().swap(cls);
    }
}
int main()
{
}